package moro;

/**
 * methoden om de route voor de Robot te berekenen
 * zoals vrije doorgang 
 * de afstand tot muren etc
 * 
 * er wordt gebruik gemaakt van de al in kaart gebrachte 
 * Occupancy Map en niet van metingen
 * het aantal vrije blokken in bekend gebied zijn bepalend
 * voor de bewegingen van de robot
 * dit is uiteraard recht evenredig met de vrije afstand tot obtacles
 * 
 * @author Bleker, van Zutphen, Stellingwerf, Grobben
 */
public class MapCalculator {

    OccupancyMap ocmap;
    Robot robot;
    int scanRange = 500;
    int checkArea = 100;

    public MapCalculator(OccupancyMap ocmap, Robot robot) {
        this.ocmap = ocmap;
        this.robot = robot;
    }

    /**
     * Meet de vrije afstand aan de voorkant van de robot met een beamwidth 
     * van 40
     * 
     * @param position Positie van het middelpunt van de robot in het grid
     * @return Afstand tot eerste obstakel
     */
    public double getDistanceFront(double position[]) {
        return getFreeDistance(position, 0, 40) - robot.getCorrF();
    }

    /**
     * Meet de vrije afstand aan de linkerkant van de robot met een beamwidth 
     * van 40
     * 
     * @param position Positie van het middelpunt van de robot in het grid
     * @return Afstand tot eerste obstakel
     */
    public double getDistanceLeft(double position[]) {
        System.out.println(getFreeDistance(position, 270, 40, 10) - robot.getCorrRL());
        return getFreeDistance(position, 270, 40, 10) - robot.getCorrRL();
    }

    public double getDistanceLeft90(double position[]) {
        System.out.println(getFreeDistance(position, 270, 40) - robot.getCorrRL());
        return getFreeDistance(position, 270, 40) - robot.getCorrRL();
    }

    /**
     * Meet de vrije afstand aan de rechterkant van de robot met een beamwidth 
     * van 40
     * 
     * @param position Positie van het middelpunt van de robot in het grid
     * @return Afstand tot aan eerste obstakel
     */
    public double getDistanceRight(double position[]) {
        return getFreeDistance(position, 90, 40, -10) - robot.getCorrRL();
    }

    public double getDistanceRight90(double position[]) {
        return getFreeDistance(position, 90, 40, -10) - robot.getCorrRL();
    }

    /**
     * Meet de vrije afstand aan de achterkant van de robot met een beamwidth 
     * van 40
     * 
     * @param position Positie van het middelpunt van de robot in het grid
     * @return Afstand tot eerste obstakel
     */
    public double getDistanceBack(double position[]) {
        return getFreeDistance(position, 180, 40) - robot.getCorrB();
    }

    /**
     * Geeft een meting recht vooruit met beamwidth 0
     * 
     * @param position Positie van de robot
     * @return Geeft afstand tot eerste obstakel
     */
    public int getFreeDistance(double[] position) {
        return getFreeDistance(position, 0, 0);
    }

    /**
     * Geeft een meting in willekeurige richting met beamwidth 0
     * 
     * @param position Positie van de robot
     * @param offset De afwijking in graden ten opzichte van recht vooruit 
     * meten
     * @return Geeft afstand tot eerste obstakel
     */
    public int getFreeDistance(double[] position, int offset) {
        return getFreeDistance(position, offset, 0);
    }

    /**
     * Meet de afstand vanaf het centrum van de robot. Er kan in verschillende
     * richtingen worden gemeten door middel van een offset. De breedte van de
     * meetstraal wordt ingesteld via de beamwidth, hiermee wordt aangegeven
     * hoevel vakjes er in de breedte gemeten moeten worden. Een beamwidth van 
     * 0 meet alleen het middelpunt van de robot in de aangegegen directie. 
     * Afstanden worden gemeten tot het eerste obstakel
     * 
     * offset 0   = recht vooruit
     * offset 90  = rechts
     * offset 180 = naar achteren
     * offset 270 = naar links
     * 
     * @param position Positie van het middelpunt van de robot in het grid
     * @param offset De afwijking in graden ten opzichte van recht vooruit meten
     * @param beamWidth De breedte van de meetstraal 
     * @return De gemeten afstand tot het eerst gevonden obstakel. 
     */
    public int getFreeDistance(double[] position, int offset, int beamWidth) {
        int xPos = (int) Math.round(position[0]);
        int yPos = (int) Math.round(position[1]);
        //Offset voor metingen in verschillende richtingen.
        //90 rechts / 180 achter / 270 links
        double measureDirection = (position[2] + offset) % 360;
        double r = Math.toRadians(measureDirection);  // graden naar RAD
        char[][] grid = ocmap.getGrid();
        int distance;

        for (distance = 0; distance <= scanRange; distance += ocmap.getCellDim()) {
            for (int x = 0 - beamWidth / 2; x <= beamWidth / 2; x += ocmap.getCellDim()) {

                int xCheck = (int) Math.round(xPos + Math.sin(r) * x + Math.cos(r) * distance);
                int yCheck = (int) Math.round(yPos + Math.cos(r) * x + Math.sin(r) * distance);

                if (xCheck < 0 || xCheck > ocmap.getWidth() || yCheck < 0 || yCheck > ocmap.getHeight()) {
                    // niks doen, meting valt buiten grid
                } else if (grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] != OccupancyMap.getEMPTY()) {
                    return distance;
                }
            }
        }
        return distance;
    }

    public int getFreeDistance(double[] position, int offset, int beamWidth, int beamOffset) {
        int xPos = (int) Math.round(position[0]);
        int yPos = (int) Math.round(position[1]);
        //Offset voor metingen in verschillende richtingen.
        //90 rechts / 180 achter / 270 links
        double measureDirection = (position[2] + offset) % 360;
        double r = Math.toRadians(measureDirection);  // graden naar RAD
        char[][] grid = ocmap.getGrid();
        int distance;

        System.out.println("Meting -----------------------------------------");
        System.out.println(measureDirection);

        for (distance = 0; distance <= scanRange; distance += ocmap.getCellDim()) {
            for (int x = 0 - beamWidth / 2; x <= beamWidth / 2; x += ocmap.getCellDim()) {

                int xCheck = (int) Math.round(xPos - Math.sin(r) * (x + beamOffset) + Math.cos(r) * distance);
                int yCheck = (int) Math.round(yPos - Math.cos(r) * (x - beamOffset) + Math.sin(r) * distance);

                System.out.println("x: " + xPos + " y: " + yPos + " xcheck " + xCheck + " ycheck " + yCheck);

                if (xCheck < 0 || xCheck > ocmap.getWidth() || yCheck < 0 || yCheck > ocmap.getHeight()) {
                    // niks doen, meting valt buiten grid
                } else if (grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] != OccupancyMap.getEMPTY()) {
                    return distance;
                }
            }
        }
        return distance;
    }

    /**
     * Controleer of de robot nog contact heeft met een muur aan de linkerkant.
     * Zolang er contact is met een muur aan de linkerkant op een afstand
     * van de aangegeven marge kan de robot doorrijden.
     * 
     * @param position Positie van het middelpunt van de robot in het grid
     * @return true indien er contact is met een muur
     */
    public boolean checkWall(double[] position) {
        int beamWidth = 40;
        int offset = 270; //meting naar links
        int xPos = (int) Math.round(position[0]);
        int yPos = (int) Math.round(position[1]);
        double measureDirection = (position[2] + offset) % 360;
        double r = Math.toRadians(measureDirection);  // graden naar RAD
        char[][] grid = ocmap.getGrid();
        int distance = robot.corrRL + robot.getMarge();

        for (int x = 0 - beamWidth / 2; x <= beamWidth / 2; x += ocmap.getCellDim()) {

            int xCheck = (int) Math.round(xPos + Math.sin(r) * x + Math.cos(r) * distance);
            int yCheck = (int) Math.round(yPos + Math.cos(r) * x + Math.sin(r) * distance);

            //controleer of er minstens één vakje muur is 
            if (grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] == OccupancyMap.getOBSTACLE()
                    || grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] == OccupancyMap.getOPAQUEOBSTACLE()) {
                return true;
            }

        }
        return false;
    }

    /**
     * Geeft lengte van de muur. Gebruikt hiervoor positie van robot
     * richting van de muur en afstand tot de muur.
     * Telt de muur tot EMPTY of UNKNOWN grid gevonden is en geeft dit afstand terug.
     * @param position
     * @param offset
     * @param wallDist
     * @return int
     */
    public int getWallLength(double[] position, int offset, int wallDist) {
        int xPos = (int) Math.round(position[0]);
        int yPos = (int) Math.round(position[1]);
        //Offset voor metingen in verschillende richtingen.
        //90 rechts / 180 achter / 270 links
        double measureDirection = (position[2] + offset) % 360; // welke kant is links/rechts
        double r = Math.toRadians(measureDirection);  // graden naar RAD
        char[][] grid = ocmap.getGrid();
        int distance;

        System.out.println("#############################");
        System.out.println("        getWallLength        ");
        System.out.println("#############################");
        for (distance = 0; distance <= scanRange; distance += ocmap.getCellDim()) {
            System.out.println(distance);

            int xCheck = (int) Math.round(xPos - Math.sin(r) * wallDist + Math.cos(r) * distance);
            int yCheck = (int) Math.round(yPos + Math.cos(r) * wallDist + Math.sin(r) * distance);

            System.out.println("----------------");
            System.out.println("xchekc " + xCheck + " ycheck " + yCheck);
            System.out.println("----------------");


            if (grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] == OccupancyMap.getEMPTY()
                    || grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] == OccupancyMap.getUNKNOWN()) {
                return distance;
            }
        }
        return 0;
    }

    /**
     * Check of om te zien of een scan nodig is in een bepaald gebied
     * 
     * @param position Positie van het middelpunt van de robot in het grid
     * @return true indien scan benodigd is
     */
    public boolean isScanNecessary(double[] position) {
        int xPos = (int) Math.round(position[0]);
        int yPos = (int) Math.round(position[1]);
        double r = Math.toRadians(position[2]);  // graden naar RAD
        char[][] grid = ocmap.getGrid();

        for (int y = 0 - checkArea / 2; y <= checkArea / 2; y += ocmap.getCellDim()) {
            for (int x = 0 - checkArea / 2; x <= checkArea / 2; x += ocmap.getCellDim()) {

                int xCheck = (int) Math.round(xPos + Math.sin(r) * x + Math.cos(r) * y);
                int yCheck = (int) Math.round(yPos + Math.cos(r) * x + Math.sin(r) * y);

                if (xCheck < 0 || xCheck >= ocmap.getWidth() || yCheck < 0 || yCheck >= ocmap.getHeight()) {
                    // do nothing
                } else {
                    // bepaal of er aan EMPTY cellen UNKNOWN cellen grenzen in het gebied rondom de robot
                    // zo ja, is dit een reden om een SCAN uit te voeren
                    if (grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] == OccupancyMap.getEMPTY()
                            && numberOfNeighbourCells(grid, (xCheck / ocmap.getCellDim()), (yCheck / ocmap.getCellDim()), OccupancyMap.getUNKNOWN()) > 0) {
                        return true;
                    } // bepaal of er aan OBSTACLE of OBSTACLEOPAQUE cellen tenminste twee cellen van
                    // hetzelfde type grenzen waarbij OBSTACLE==OPAQUEOBSTACLE
                    // dit is dan dus als een ononderbroken keten te herkennen
                    // zo Nee is dit een reden om een SCAN uit te voeren omdat er dan een  "gat"  aanwezig is.
                    else if ((grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] == OccupancyMap.getOBSTACLE()
                            || grid[xCheck / ocmap.getCellDim()][yCheck / ocmap.getCellDim()] == OccupancyMap.getOPAQUEOBSTACLE())
                            && numberOfConnectedCells(grid, (xCheck / ocmap.getCellDim()), (yCheck / ocmap.getCellDim())) < 2) {

                        return true;
                    }
                }
            }
        }
        return false;
    }

    /**
     * Is de gehele omgeving in kaart gebracht 
     * return true indien de gehele OC map onderzocht is
     * dit wordt bepaald door het feit dat er geen EMPTY cellen meer grenzen aan 
     * cellen van het type UNKNOWN
     *  
     * @return isExplored boolean
     */
    public boolean occupancyMapIsExplored() {
        char[][] grid = ocmap.getGrid();
        boolean isExplored = false;
        int emptyCount = 0;
        for (int x = 0; x < ocmap.getWidth() / ocmap.getCellDim(); x++) {
            for (int y = 0; y < ocmap.getHeight() / ocmap.getCellDim(); y++) {
                if (grid[x][y] == OccupancyMap.getEMPTY()) {
                    if (numberOfNeighbourCells(grid, x, y, OccupancyMap.getUNKNOWN()) > 0) {
                        emptyCount++;
                    }
                }
            }
        }
        // een robot kan niet door een "gat" van minder dan 4 cellen breed
        // dit laat ruimte voor een klein scanfoutje van een obstacle
        if (emptyCount < 1) {
            isExplored = true;
        }
        return isExplored;
    }

    /**
     *  bepaal hoeveel aangrenzende cellen van het type CellType zijn
     *  niet de diagonaal verbonden cellen!
     * 
     *  @return number het aantal aangrenzende cellen type cellType
     */
    private int numberOfNeighbourCells(char[][] grid, int x, int y, char cellType) {
        int number = 0;
        try {
            // oost
            if (grid[x + 1][y] == cellType) {
                number++;
            }
            // zuid
            if (grid[x][y + 1] == cellType) {
                number++;
            }
            // west
            if (grid[x - 1][y] == cellType) {
                number++;
            }
            // noord
            if (grid[x][y - 1] == cellType) {
                number++;
            }
        } catch (IndexOutOfBoundsException e) {
            // do nothing, unimportant
        }
        return number;
    }

    /**
     *  bepaal of een OBSTACLE of OPAQUEOBSTACLE cell verbonden is met tenminste
     *  twee cellen van hetzelfde type (dus een van beide)
     *  niet de diagonaal verbonden cellen
     * 
     *  @return number het aantal aangrenzende cellen type obstacle of opaqueobstacle
     */
    private int numberOfConnectedCells(char[][] grid, int x, int y) {
        int number = 0;
        try {
            // oost
            if (grid[x + 1][y] == OccupancyMap.getOBSTACLE() || grid[x + 1][y] == OccupancyMap.getOPAQUEOBSTACLE()) {
                number++;
            }
            // zuid
            if (grid[x][y + 1] == OccupancyMap.getOBSTACLE() || grid[x][y + 1] == OccupancyMap.getOPAQUEOBSTACLE()) {
                number++;
            }
            // west
            if (grid[x - 1][y] == OccupancyMap.getOBSTACLE() || grid[x - 1][y] == OccupancyMap.getOPAQUEOBSTACLE()) {
                number++;
            }
            // noord
            if (grid[x][y - 1] == OccupancyMap.getOBSTACLE() || grid[x][y - 1] == OccupancyMap.getOPAQUEOBSTACLE()) {
                number++;
            }
        } catch (IndexOutOfBoundsException e) {
            // do nothing, unimportant
        }
        return number;
    }

    /**
     * Is de gehele omgeving in kaart gebracht 
     * return true indien de gehele OC map onderzocht is
     * dit wordt bepaald door het feit dat er een ononderbroken buitengrens bestaat 
     * 
     *  
     * @return isExplored boolean
     */
    public boolean occupancyMapHasOuterBoundary() {
        boolean isExplored = false;

        char[][] grid = ocmap.getGrid();
        // creer een controle grid van dezelfde afmeting
        int[][] checkGrid = new int[ocmap.getWidth() / ocmap.getCellDim()][ocmap.getHeight() / ocmap.getCellDim()];
        // initialiseer controlegrid
        for (int x = 0; x < ocmap.getWidth() / ocmap.getCellDim(); x++) {
            for (int y = 0; y < ocmap.getHeight() / ocmap.getCellDim(); y++) {
                checkGrid[x][y] = 0;
            }
        }

        // zoek de eerste cel in de OCmap die een grenst aan EMPTY gebied        
        int refX = -1;
        int refY = -1;
        Boolean found = false;
        for (int x = 0; x < (ocmap.getWidth() / ocmap.getCellDim()); x++) {
            for (int y = 0; (y < ocmap.getHeight() / ocmap.getCellDim()); y++) {
                if (grid[x][y] == OccupancyMap.getOBSTACLE() || grid[x][y] == OccupancyMap.getOPAQUEOBSTACLE()) {
                    if (numberOfNeighbourCells(grid, x, y, OccupancyMap.getEMPTY()) > 0) {
                        if (!found) {
                            refX = x;
                            refY = y;
                            found = true;
                            // markeer startpunt
                            checkGrid[refX][refY] = 1;
                        }
                    }
                }
            }
        }
        // System.out.println("chain 1e: " + refX + " , " + refY + " : " + isExplored);
        // geen "eerste" cel gevonden
        if (refX < 0 && refY < 0) {
            return isExplored;
        }

        // probeer nu de keten van de "muur" te volgen
        // dit had een recursieve methode kunnen zijn maar daar houd ik niet van  (Henk)
        boolean lookForChain = true;
        int[] nextXY = new int[2];
        int nextX = refX;
        int nextY = refY;
        int numberOfCells = 0;

        while (lookForChain == true) {
            nextXY = getNextCell(grid, checkGrid, nextX, nextY, numberOfCells);
            numberOfCells++;
            // System.out.println("chain: " + nextXY[0] + " , " + nextXY[1] + " : _  refxy= " + refX + " " + refY +  " val=" + checkGrid[refX][refY] + " lfc: " + lookForChain);
            // geen volgende cell gevonden
            if (nextXY[0] < 0 && nextXY[1] < 0) {
                lookForChain = false;
                // terug op uitgangspunt
            } else if (nextXY[0] == refX && nextXY[1] == refY) {
                lookForChain = false;
                isExplored = true;
                // volgende cel in keten gevonden, markeren
            } else {
                checkGrid[nextXY[0]][nextXY[1]] = 2;
                nextX = nextXY[0];
                nextY = nextXY[1];
            }
        }
        System.out.println("chainexit: " + isExplored);
        return isExplored;
    }

    /**
     * Zoek alle aangrenzende cellen van hetzelfde type, ook diagonaal
     * die grenzen aan een EMPTY cell en probeer de "keten"  van de buitengrens
     * te volgen.
     * @param grid
     * @param checkGrid
     * @param xz
     * @param yz
     * @return nextCell de volgende cel in de keten 
     */
    private int[] getNextCell(char[][] grid, int[][] checkGrid, int xz, int yz, int chainLength) {
        int[] nextCell = new int[2];
        nextCell[0] = -1;
        nextCell[1] = -1;
        boolean found = false;

        try {
            for (int x = xz - 1; x < (xz + 2); x++) {
                for (int y = yz - 1; y < (yz + 2); y++) {
                    // sla de referentie cel zelf over
                    if (x == xz && y == yz) {
                    } else if (x < 0 || x > (ocmap.getWidth() / ocmap.getCellDim() - 1)) {
                    } else if (y < 0 || y > (ocmap.getHeight() / ocmap.getCellDim() - 1)) {
                    } else if (grid[x][y] == OccupancyMap.getOBSTACLE() || grid[x][y] == OccupancyMap.getOPAQUEOBSTACLE()) {
                        if (numberOfNeighbourCells(grid, x, y, OccupancyMap.getEMPTY()) > 0) {
                            // alleen nog niet gemarkeerde cellen -> checkGrid moet 0 of 1 (=start) zijn
                            if (!(found) && checkGrid[x][y] == 0) {
                                nextCell[0] = x;
                                nextCell[1] = y;
                                found = true;
                                // weer bij het begin van de keten aangekomen
                            } else if (!(found) && checkGrid[x][y] == 1 && chainLength > 9) {
                                nextCell[0] = x;
                                nextCell[1] = y;
                                found = true;
                            }
                        }
                    }
                }
            }
        } catch (IndexOutOfBoundsException e) {
            // do nothing, unimportant
        }
        return nextCell;
    }
}
